#include "PiecewiseLinearInterpolation.hpp"
#include "stdio.h"
PiecewiseLinearInterpolation::PiecewiseLinearInterpolation(){
    this->x_set = nullptr;
    this->y_set = nullptr;
    this->n_point = 0;
}

PiecewiseLinearInterpolation::~PiecewiseLinearInterpolation(){
    if ( this->x_set != nullptr ){
        delete[] this->x_set;
        this->x_set = nullptr;
    }
    if ( this->y_set != nullptr ){
        delete[] this->y_set;
        this->y_set = nullptr;
    }
}

bool PiecewiseLinearInterpolation::load(float *x_set,float *y_set,int n_point){
    if ( this->x_set != nullptr ){
        delete[] this->x_set;
        this->x_set = nullptr;
    }
    if ( this->y_set != nullptr ){
        delete[] this->y_set;
        this->y_set = nullptr;
    }
    this->x_set = new float[n_point];
    this->y_set = new float[n_point];
    this->n_point = n_point;
    for ( int index = 0; index < n_point; index++ ){
        this->x_set[index] = x_set[index];
        this->y_set[index] = y_set[index];
    }
    /* Sort */
    float temp = 0;
    for ( int m = 0; m < n_point - 1; m++ ){
        for ( int n = 0; n < n_point - 1; n++ ){
            if ( x_set[n] > x_set[n + 1] ){
                temp = x_set[n];
                x_set[n] = x_set[n + 1];
                x_set[n + 1] = temp;
                temp = y_set[n];
                y_set[n] = y_set[n + 1];
                y_set[n + 1] = temp;
            }
        }
    }
    return true;
}

bool PiecewiseLinearInterpolation::check(float x){
    if ( x < this->x_set[0] || x > this->x_set[this->n_point - 1] ) return false;
    else return true;
}

float PiecewiseLinearInterpolation::calculate(float x){
    // if ( !this->check(x) ) return 0;
    /*new add*/

    if ( !this->check(x) ) return 0;

    int start_range = 0;
    for ( start_range = 0; start_range < this->n_point; start_range++ ){
        if ( this->x_set[start_range] <= x && this->x_set[start_range + 1] >= x ){
            break;
        }
    }
    if (start_range == this->n_point - 1) {
        return y_set[start_range-1]; 
    }
    return y_set[start_range] + (y_set[start_range + 1] - y_set[start_range]) * (x - x_set[start_range]) / (x_set[start_range + 1] - x_set[start_range]);
}

void PiecewiseLinearInterpolation::debugPrint(void){
    printf("Piecewise Linear Interpolation\r\n------------\r\n");
    printf("Point count:%d\r\n",this->n_point);
    for ( int n = 0; n < this->n_point; n++ ){
        printf("P(%d) = (%f,%f)\r\n",n,this->x_set[n],this->y_set[n]);
    }
}
